﻿#include "MeCvMath.h"



int MeCvMath::meanStdDevPoints(const std::vector<cv::Point3d> &points, cv::Point3d &meanP, double &mean, double &stddev,
                                  double &maxV)
{
    uint count = points.size();
    if (count == 0) {
        return -1;
    }

    double scale = 1. / (count);

    meanP = {0,0,0};
    for (uint i=0; i< points.size(); i++) {
        meanP += points[i];
    }
    meanP *= scale;

    double sum = { 0. }, sqsum = { 0. };
    maxV = 0;
    for (unsigned int i = 0; i < count; ++i) {
        double v = MeCvMath::distanceFrom2p(points[i], meanP);
        sum += v;
        sqsum += v * v;
        if (v > maxV) {
            maxV = v;
        }
    }
    mean = sum * scale;

    // 所有距离平方/n 开方
    double variance = std::max(sqsum*scale, 0.);
    stddev = std::sqrt(variance);
    return 0;
}

void MeCvMath::multiplyPoint3f(Matrix4d &m, const cv::Point3f &vec, cv::Point3f &outP)
{
    double v1 = vec.x;
    double v2 = vec.y;
    double v3 = vec.z;
    double v4 = 1;

    outP.x = v1*m[0][0] + v2*m[0][1] + v3*m[0][2] + v4*m[0][3];
    outP.y = v1*m[1][0] + v2*m[1][1] + v3*m[1][2] + v4*m[1][3];
    outP.z = v1*m[2][0] + v2*m[2][1] + v3*m[2][2] + v4*m[2][3];
}

Matrix4d MeCvMath::toRotMatrix4d(cv::Mat &R) {
    Matrix4d m;
    m[0][0] = R.at<double>(0,0);
    m[1][0] = R.at<double>(1,0);
    m[2][0] = R.at<double>(2,0);
    m[3][0] = 0;

    m[0][1] = R.at<double>(0,1);
    m[1][1] = R.at<double>(1,1);
    m[2][1] = R.at<double>(2,1);
    m[3][1] = 0;

    m[0][2] = R.at<double>(0,2);
    m[1][2] = R.at<double>(1,2);
    m[2][2] = R.at<double>(2,2);
    m[3][2] = 0;

    m[0][3] = 0;
    m[1][3] = 0;
    m[2][3] = 0;
    m[3][3] = 1;

    return m;
}

Matrix4d MeCvMath::toMatrix4d(cv::Mat &R, cv::Mat &t)
{
    Matrix4d m;
    m[0][0] = R.at<double>(0,0);
    m[1][0] = R.at<double>(1,0);
    m[2][0] = R.at<double>(2,0);
    m[3][0] = 0;

    m[0][1] = R.at<double>(0,1);
    m[1][1] = R.at<double>(1,1);
    m[2][1] = R.at<double>(2,1);
    m[3][1] = 0;

    m[0][2] = R.at<double>(0,2);
    m[1][2] = R.at<double>(1,2);
    m[2][2] = R.at<double>(2,2);
    m[3][2] = 0;

    double *pt = t.ptr<double>();
    m[0][3] = pt[0];
    m[1][3] = pt[1];
    m[2][3] = pt[2];
    m[3][3] = 1;

    return m;
}
